#include "imu.h"

/*
 * 功能: 初始化IMU设备并设置采样周期
 * 参数:
 *   imu_name - IMU设备名称，必须与Webots中的IMU节点名称一致
 *   sampling_period - 采样周期，单位为毫秒(ms)，若为0则使用默认值
 * 返回: IMU设备句柄
 */
WbDeviceTag IMU_Init(const char *imu_name, int sampling_period)
{

    // 使用默认采样周期如果参数为0
    if (sampling_period == 0)
    {
        sampling_period = IMU_SAMPLING_PERIOD_DEFAULT;
    }

    // 获取IMU句柄
    WbDeviceTag imu_handle = wb_robot_get_device(imu_name);

    // 使能IMU并设置采样周期
    wb_inertial_unit_enable(imu_handle, sampling_period);

    return imu_handle;
}

/*
 * 功能: 获取IMU的XYZ值
 * 参数:
 *   imu - IMU设备句柄
 *   x - X轴值指针
 *   y - Y轴值指针
 *   z - Z轴值指针
 * 返回: 无
 */
void IMU_Get_XYZ_Values(WbDeviceTag imu, double *x, double *y, double *z)
{
    const double *xyz = wb_inertial_unit_get_roll_pitch_yaw(imu);
    if (x)
        *x = xyz[0];
    if (y)
        *y = xyz[1];
    if (z)
        *z = xyz[2];
}

/*
 * 功能: 获取IMU的XYZ值（单位: °）
 * 参数:
 *   imu - IMU设备句柄
 *   x - X轴值指针（单位: °）
 *   y - Y轴值指针（单位: °）
 *   z - Z轴值指针（单位: °）
 * 返回: 无
 */
void IMU_Get_XYZ_Values_Degrees(WbDeviceTag imu, double *x, double *y, double *z)
{

    const double *xyz = wb_inertial_unit_get_roll_pitch_yaw(imu);
    if (xyz)
    {

        double roll = xyz[0];
        double pitch = xyz[1];
        double yaw = xyz[2];

        // 转换为角度
        if (x)
            *x = roll * (180.0 / M_PI);
        if (y)
            *y = pitch * (180.0 / M_PI);
        if (z)
            *z = yaw * (180.0 / M_PI);
    }
}
